#include <CameraManipulatorIntersection.h>
#include <osg/MatrixTransform>
#include <osg/LineWidth>
#include <iostream>
#include <osgGA/GUIEventAdapter>
#include <osg/Math>
#include <osgUtil/LineSegmentIntersector>
#include <osg/ComputeBoundsVisitor>
CameraManipulatorIntersection::CameraManipulatorIntersection()
{
    _distance = 20;

    _rotation_matrix = osg::Matrix::rotate(osg::PI*3/4, osg::Vec3(0,0,1))*osg::Matrix::rotate(osg::Vec3(0,0,-1), osg::Vec3(-1,-1,-1));
    _pushed_mouse_button_mask = -1;


}

CameraManipulatorIntersection::~CameraManipulatorIntersection()
{

}



void CameraManipulatorIntersection::setByMatrix(const osg::Matrixd &matrix)
{

}

void CameraManipulatorIntersection::setByInverseMatrix(const osg::Matrixd &matrix)
{

}

osg::Matrixd CameraManipulatorIntersection::getMatrix() const
{
    osg::Matrix m;

    m = osg::Matrixd::translate( 0., 0., _distance )
            *_rotation_matrix
            *osg::Matrixd::translate( _center );


    return m;
}

osg::Matrixd CameraManipulatorIntersection::getInverseMatrix() const
{
    //osg::Matrix m ;

    return osg::Matrix::inverse(getMatrix());

}

bool CameraManipulatorIntersection::handle(const osgGA::GUIEventAdapter &ea, osgGA::GUIActionAdapter &us)
{

    switch( ea.getEventType() )
    {

    case osgGA::GUIEventAdapter::FRAME:
        return handleFrame( ea, us );

    case osgGA::GUIEventAdapter::RESIZE:
        return handleResize( ea, us );

    default:
        break;
    }

    if( ea.getHandled() )
        return false;

    switch( ea.getEventType() )
    {
    case osgGA::GUIEventAdapter::MOVE:
        return handleMouseMove( ea, us );

    case osgGA::GUIEventAdapter::DRAG:
        return handleMouseDrag( ea, us );

    case osgGA::GUIEventAdapter::PUSH:
        return handleMousePush( ea, us );

    case osgGA::GUIEventAdapter::RELEASE:
        return handleMouseRelease( ea, us );

    case osgGA::GUIEventAdapter::KEYDOWN:
        return handleKeyDown( ea, us );

    case osgGA::GUIEventAdapter::KEYUP:
        return handleKeyUp( ea, us );

    case osgGA::GUIEventAdapter::SCROLL:
        return handleMouseWheel( ea, us );
    default:
        return false;
    }

}

void CameraManipulatorIntersection::home(const osgGA::GUIEventAdapter &ea, osgGA::GUIActionAdapter &us)
{
    //std::cout<<"*****home*****"<<std::endl;
    if( getAutoComputeHomePosition() )
    {
        const osg::Camera *camera = us.asView() ? us.asView()->getCamera() : NULL;
        computeHomePosition( camera, false );
    }
}

void CameraManipulatorIntersection::computeHomePosition(const osg::Camera *camera, bool useBoundingBox)
{
    //std::cout<<"*****computeHomePosition*****"<<std::endl;
    if (getNode())
    {
        osg::BoundingSphere boundingSphere;

        //OSG_INFO<<" CameraManipulator::computeHomePosition("<<camera<<", "<<useBoundingBox<<")"<<std::endl;

        if (useBoundingBox)
        {
            // compute bounding box
            // (bounding box computes model center more precisely than bounding sphere)
            osg::ComputeBoundsVisitor cbVisitor;
            getNode()->accept(cbVisitor);
            osg::BoundingBox &bb = cbVisitor.getBoundingBox();

            if (bb.valid()) boundingSphere.expandBy(bb);
            else boundingSphere = getNode()->getBound();
        }
        else
        {
            // compute bounding sphere
            boundingSphere = getNode()->getBound();
        }

        //OSG_INFO<<"    boundingSphere.center() = ("<<boundingSphere.center()<<")"<<std::endl;
        //OSG_INFO<<"    boundingSphere.radius() = "<<boundingSphere.radius()<<std::endl;

        _distance = 3.5f * boundingSphere.radius();
        _rotation_matrix = osg::Matrix::rotate(osg::PI*3/4, osg::Vec3(0,0,1))*osg::Matrix::rotate(osg::Vec3(0,0,-1), osg::Vec3(-1,-1,-1));
        _center = boundingSphere.center();
    }


}

void CameraManipulatorIntersection::setNode(osg::Node *node)
{
    _node = node;

    // compute home position
    if( getAutoComputeHomePosition() )
        computeHomePosition( NULL, false );
    //std::cout<<"setNode"<<std::endl;
}

const osg::Node *CameraManipulatorIntersection::getNode() const
{
    return _node.get();
}

osg::Node *CameraManipulatorIntersection::getNode()
{
    return _node.get();
}

bool CameraManipulatorIntersection::handleFrame(const osgGA::GUIEventAdapter &ea, osgGA::GUIActionAdapter &us)
{
    return false;
}

bool CameraManipulatorIntersection::handleResize(const osgGA::GUIEventAdapter &ea, osgGA::GUIActionAdapter &us)
{
    return false;
}

bool CameraManipulatorIntersection::handleMouseMove(const osgGA::GUIEventAdapter &ea, osgGA::GUIActionAdapter &us)
{
    _last_x = ea.getXnormalized();
    _last_y = ea.getYnormalized();

    return false;
}

bool CameraManipulatorIntersection::handleMouseDrag(const osgGA::GUIEventAdapter &ea, osgGA::GUIActionAdapter &us)
{

    if(_pushed_mouse_button_mask == osgGA::GUIEventAdapter::LEFT_MOUSE_BUTTON)
    {
        handleMouseDragLeft(ea, us);
    }
    else if(_pushed_mouse_button_mask == osgGA::GUIEventAdapter::MIDDLE_MOUSE_BUTTON)
    {
        handleMouseDragMiddle(ea, us);
    }
    if(_pushed_mouse_button_mask == osgGA::GUIEventAdapter::RIGHT_MOUSE_BUTTON)
    {

    }

    _last_x = ea.getXnormalized();
    _last_y = ea.getYnormalized();

    return false;
}

bool CameraManipulatorIntersection::handleMousePush(const osgGA::GUIEventAdapter &ea, osgGA::GUIActionAdapter &us)
{

    _pushed_mouse_button_mask = ea.getButtonMask();

    if(_pushed_mouse_button_mask == osgGA::GUIEventAdapter::LEFT_MOUSE_BUTTON)
        handleMousePushIntersection(ea, us);
    return false;
}

bool CameraManipulatorIntersection::handleMouseRelease(const osgGA::GUIEventAdapter &ea, osgGA::GUIActionAdapter &us)
{
    _pushed_mouse_button_mask = -1;
    handleMouseReleaseIntersection(ea, us);
    return false;
}

bool CameraManipulatorIntersection::handleKeyDown(const osgGA::GUIEventAdapter &ea, osgGA::GUIActionAdapter &us)
{
    if( ea.getKey() == osgGA::GUIEventAdapter::KEY_Space )
    {
        home(ea,us);
        return true;
    }

    return false;
}

bool CameraManipulatorIntersection::handleKeyUp(const osgGA::GUIEventAdapter &ea, osgGA::GUIActionAdapter &us)
{
    return false;
}

bool CameraManipulatorIntersection::handleMouseWheel(const osgGA::GUIEventAdapter &ea, osgGA::GUIActionAdapter &us)
{
    osgGA::GUIEventAdapter::ScrollingMotion sm = ea.getScrollingMotion();

    switch( sm )
    {
    // mouse scroll up event
    case osgGA::GUIEventAdapter::SCROLL_UP:
    {
        zoom(0.1*_distance+0.01);
        us.requestRedraw();

        return true;
    }

        // mouse scroll down event
    case osgGA::GUIEventAdapter::SCROLL_DOWN:
    {
        if(_distance > 10e-6)
            zoom(-0.1*_distance);
        us.requestRedraw();

        return true;
    }

        // unhandled mouse scrolling motion
    default:
        return false;
    }

    return false;
}

bool CameraManipulatorIntersection::handleMouseDeltaMovement(const osgGA::GUIEventAdapter &ea, osgGA::GUIActionAdapter &us)
{
    float x = ea.getXnormalized();
    float y = ea.getYnormalized();

    return false;
}

bool CameraManipulatorIntersection::handleMouseDragLeft(const osgGA::GUIEventAdapter &ea, osgGA::GUIActionAdapter &us)
{
    float x = ea.getXnormalized();
    float y = ea.getYnormalized();

    rotateTrackball( x, y, _last_x, _last_y  );

    return false;
}

bool CameraManipulatorIntersection::handleMouseDragMiddle(const osgGA::GUIEventAdapter &ea, osgGA::GUIActionAdapter &us)
{
    float x = ea.getXnormalized();
    float y = ea.getYnormalized();
    float scale;
    if(_distance < -10e-6)
        scale = 1.0;
    else if(_distance < 10e-6)
        scale = 1.0;
    else
        scale = 0.3f * _distance;

    pan((_last_x - x )*scale, (_last_y - y )*scale);
    return false;
}

bool CameraManipulatorIntersection::handleMousePushIntersection(const osgGA::GUIEventAdapter &ea, osgGA::GUIActionAdapter &us)
{
    _insert_center = osg::Vec3d(0,0,0);
    osg::View* view = us.asView();
    if( !view )
        return false;

    osg::Camera *camera = view->getCamera();
    if( !camera )
        return false;

    // prepare variables
    float x = ( ea.getX() - ea.getXmin() ) / ( ea.getXmax() - ea.getXmin() );
    float y = ( ea.getY() - ea.getYmin() ) / ( ea.getYmax() - ea.getYmin() );
    osgUtil::LineSegmentIntersector::CoordinateFrame cf;
    osg::Viewport *vp = camera->getViewport();
    if( vp ) {
        cf = osgUtil::Intersector::WINDOW;
        x *= vp->width();
        y *= vp->height();
    } else
        cf = osgUtil::Intersector::PROJECTION;

    // perform intersection computation
    osg::ref_ptr< osgUtil::LineSegmentIntersector > picker = new osgUtil::LineSegmentIntersector( cf, x, y );
    osgUtil::IntersectionVisitor iv( picker.get() );
    camera->accept( iv );

    // return on no intersections
    if( !picker->containsIntersections() )
        return false;

    // get all intersections
    osgUtil::LineSegmentIntersector::Intersections& intersections = picker->getIntersections();

    // get current transformation
    //osg::Vec3d eye, oldCenter, up;
    //getTransformation( eye, oldCenter, up );

    // new center
    osg::Vec3d new_center = (*intersections.begin()).getWorldIntersectPoint();


    //_insert_center = new_center;

    _insert_center =  new_center;//getMatrix() *


    //add point
    createInsertPoint(new_center);
    camera->addChild(_insert_point);

    return false;
}

bool CameraManipulatorIntersection::handleMouseReleaseIntersection(const osgGA::GUIEventAdapter &ea, osgGA::GUIActionAdapter &us)
{
    osg::View* view = us.asView();
    if( !view )
        return false;

    osg::Camera *camera = view->getCamera();
    if( !camera )
        return false;

    camera->removeChild(_insert_point);
}

void CameraManipulatorIntersection::zoom(const float interval)
{
    _distance += interval;
}
void CameraManipulatorIntersection::pan( const float dx, const float dy )
{
    osg::Vec3d dv( dx, dy, 0 );
    osg::Matrixd rotation_matrix(_rotation_matrix.getRotate());
    _center += dv * rotation_matrix;
}

void CameraManipulatorIntersection::rotateTrackball(const float px0, const float py0,
                                                    const float px1, const float py1)
{
    osg::Vec3d axis;
    float angle;

    trackball( axis, angle, px1, py1, px0, py0 );

    osg::Quat new_rotate;
    new_rotate.makeRotate( angle, axis );

    osg::Matrix new_rotate_matrix = osg::Matrix::translate(_center)
            *osg::Matrix::translate(_insert_center*-1.0)
            * osg::Matrix::rotate(new_rotate)
            * osg::Matrix::translate(_insert_center)
            * osg::Matrix::translate(_center*-1.0);

    _rotation_matrix = _rotation_matrix * new_rotate_matrix;
}

void CameraManipulatorIntersection::trackball( osg::Vec3d& axis, float& angle, float p1x, float p1y, float p2x, float p2y )
{
    /*
        * First, figure out z-coordinates for projection of P1 and P2 to
        * deformed sphere
        */
    float _trackballSize = 0.8;
    //osg::Matrixd rotation_matrix(_rotation);
    osg::Matrixd rotation_matrix(_rotation_matrix.getRotate());

    osg::Vec3d uv = osg::Vec3d(0.0f,1.0f,0.0f)*rotation_matrix;
    osg::Vec3d sv = osg::Vec3d(1.0f,0.0f,0.0f)*rotation_matrix;
    osg::Vec3d lv = osg::Vec3d(0.0f,0.0f,-1.0f)*rotation_matrix;

    osg::Vec3d p1 = sv * p1x + uv * p1y - lv * tb_project_to_sphere(_trackballSize, p1x, p1y);
    osg::Vec3d p2 = sv * p2x + uv * p2y - lv * tb_project_to_sphere(_trackballSize, p2x, p2y);

    /*
        *  Now, we want the cross product of P1 and P2
        */
    axis = p2^p1;
    axis.normalize();

    /*
        *  Figure out how much to rotate around that axis.
        */
    float t = (p2 - p1).length() / (2.0 * _trackballSize);

    /*
        * Avoid problems with out-of-control values...
        */
    if (t > 1.0) t = 1.0;
    if (t < -1.0) t = -1.0;

    angle = osg::inRadians(asin(t));
}


float CameraManipulatorIntersection::tb_project_to_sphere( float r, float x, float y )
{
    float d, t, z;

    d = sqrt(x*x + y*y);
    /* Inside sphere */
    if (d < r * 0.70710678118654752440)
    {
        z = sqrt(r*r - d*d);
    }                            /* On hyperbola */
    else
    {
        t = r / 1.41421356237309504880;
        z = t*t / d;
    }
    return z;
}

void CameraManipulatorIntersection::createInsertPoint(osg::Vec3d &pos)
{
    const std::string message = "+";
    std::string font_path("fonts/arial.ttf");
    float char_Size = 25.0;
    float min_scale = 0.0;
    float max_scale = FLT_MAX;//5.0;
    float point_size = char_Size*0.5;
    float line_width = 2;
    float point_detail_ratio = 0.5;
    osg::Vec3 text_pos(0,0,40*char_Size);//(pos.x(), pos.y(), pos.z()+40*char_Size);

    //auto trans
    osg::ref_ptr<osg::AutoTransform> auto_trans = new osg::AutoTransform;
    auto_trans->setAutoRotateMode(osg::AutoTransform::ROTATE_TO_SCREEN);
    auto_trans->setAutoScaleToScreen(true);
    auto_trans->setMinimumScale(min_scale);
    auto_trans->setMaximumScale(max_scale);
    auto_trans->setPosition(pos);

    //geode
    osg::Geode* geode = new osg::Geode;
    geode->getOrCreateStateSet()->setMode(GL_LIGHTING, osg::StateAttribute::OFF);
    auto_trans->addChild(geode);

    //text
    osgText::Text* text = new osgText::Text;
    text->setCharacterSize(char_Size);
    text->setText(message);
    //text->setFont(font_path);
    text->setAlignment(osgText::Text::CENTER_CENTER);
    //text->setPosition(text_pos);
    geode->addDrawable(text);

    _insert_point = auto_trans;
}
